import asyncio
import time
from mavsdk import System

async def run():

    drone = System()
    await drone.connect(system_address="udpin://0.0.0.0:14540")

    ground = System()
    await ground.connect(system_address="udpout://127.0.0.1:14551")

    '''
    Обработка подключения к FC
    '''
    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"-- Connected to drone!")
            break

    '''
    Обработка подключения к GCS
    '''
    print("Waiting for ground to connect...")
    async for state in ground.core.connection_state():
        if state.is_connected:
            print(f"-- Connected to ground!")
            break

    '''
    Предстартовая проверка
    '''
    print("Waiting for drone to have a global position estimate...")
    async for health in drone.telemetry.health():
        if health.is_global_position_ok and health.is_home_position_ok:
            print("-- Global position estimate OK")
            break

    asyncio.ensure_future(proxy_imu(drone, ground))

    while True:
        await asyncio.sleep(1)


async def proxy_imu(drone, ground):
    async for imu in drone.telemetry.imu():
        print(imu)
        ground.telemetry_server.publish_imu(imu);

if __name__ == "__main__":
    asyncio.run(run())
